#include "fsm.h"

#include "fsm/vehicle/workingstate.h"
#include "fsm/vehicle/resetstate.h"
#include "fsm/vehicle/sleepstate.h"
#include "fsm/vehicle/pausestate.h"
#include "fsm/vehicle/preparestate.h"
#include "basestate.h"
#include "masters/vehicle_controller.h"
using namespace std;
using namespace vehicle;

FSM::FSM(VehicleController *vehicle_controller)
{
    vehicle_controller_=vehicle_controller;

    sleep_state_=new SleepState();
    prepare_state_=new PrepareState();
    reset_state_=new ResetState();
    working_state_=new WorkingState();
    pause_state_=new PauseState();
    cur_state_=sleep_state_;
    cur_state_->OnEntry(this);
}

FSM::~FSM()
{
    //delete device_;

    delete sleep_state_;
    delete prepare_state_;
    delete reset_state_;
    delete working_state_;
    delete pause_state_;
}

void FSM::SetState(BaseState::StateType state)
{
    //ForceMode,PositionMode,IdleMode,ReadyMode,PauseMode
    if(cur_state_->GetState()==state)
        return;
    switch (state) {
    case BaseState::kResetState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=reset_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kWorkingState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=working_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kSleepState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        vehicle_controller_->StopRobotRC();
        cur_state_=sleep_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kPrepareState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=prepare_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kPauseState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=pause_state_;
        cur_state_->OnEntry(this);
        break;
    default:
        cout<<"unkown state"<<endl;
        break;
    }

}





//bool FSM::SetSlaveTargetPos(const double target_px, const double target_py, const double target_pz)
//{
//    remote_controller_->SendTargetPos2Robot(target_px,target_py,target_pz);
//    return true;
//}

//bool FSM::SetMasterTargetPos(const double target_px, const double target_py, const double target_pz)
//{
//    master_target_pos[0]=target_px;
//    master_target_pos[1]=target_py;
//    master_target_pos[2]=target_pz;
//    return true;
//}



void FSM::Update()
{
    //string states[]={"ForceState","PositionState","IdleState","ReadyState","PauseState"};
    cur_state_->Update(this);
    //cout<<"cur_state:"<<cur_state_->GetState()<<endl;
}
